#ifndef MAT4_HPP
#define MAT4_HPP
#include "Vec3.hpp"
#include "Vec2.hpp"
#include "Math.hpp"
class Mat4
{
public:
   typedef float T;
   Mat4()
   {
      set(1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1);   
   }
   static Mat4 zero()
   {
      return Mat4(0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0);
   }

   Mat4(T a, T b, T c, T d,
         T e, T f, T g, T h,
         T i, T j, T k, T l,
         T m, T n, T o, T p)
   {
      set(a,b,c,d,e,f,g,h,i,j,k,l,m,n,o,p);
   }
    void set(T a, T b, T c, T d,
         T e, T f, T g, T h,
         T i, T j, T k, T l,
         T m, T n, T o, T p)
    {
       values[0] = a;values[1] = b; values[2] = c;values[3] = d;
       values[4] = e;values[5] = f; values[6] = g;values[7] = h;
       values[8] = i;values[9] = j; values[10] = k;values[11] = l;
       values[12] = m;values[13] = n; values[14] = o;values[15] = p;
    }

   T& operator()(int a, int b)
   {
      return values[4 * a + b];
   }
   T operator()(int a, int b) const
   {
      return values[4 * a + b];
   }
   
   Mat4 transpose() const
   {
      return Mat4(values[0], values[4], values[8], values[12],
                  values[1], values[5], values[9], values[13],
                  values[2], values[6], values[10], values[14],
                  values[3], values[7], values[11], values[15]);
   }
   T det() const
   {
       float fA0 = values[ 0]*values[ 5] - values[ 1]*values[ 4];
       float fA1 = values[ 0]*values[ 6] - values[ 2]*values[ 4];
       float fA2 = values[ 0]*values[ 7] - values[ 3]*values[ 4];
       float fA3 = values[ 1]*values[ 6] - values[ 2]*values[ 5];
       float fA4 = values[ 1]*values[ 7] - values[ 3]*values[ 5];
       float fA5 = values[ 2]*values[ 7] - values[ 3]*values[ 6];
       float fB0 = values[ 8]*values[13] - values[ 9]*values[12];
       float fB1 = values[ 8]*values[14] - values[10]*values[12];
       float fB2 = values[ 8]*values[15] - values[11]*values[12];
       float fB3 = values[ 9]*values[14] - values[10]*values[13];
       float fB4 = values[ 9]*values[15] - values[11]*values[13];
       float fB5 = values[10]*values[15] - values[11]*values[14];
       float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
       return fDet;
   }
   Mat4 operator+(Mat4 a)
   {
      return Mat4(values[0] + a.values[0], values[1] + a.values[1], values[2] + a.values[2], values[3] + a.values[3],
         values[4] + a.values[4], values[5] + a.values[5], values[6] + a.values[6], values[7] + a.values[7],
         values[8] + a.values[8], values[9] + a.values[9], values[10] + a.values[10], values[11] + a.values[11],
         values[12] + a.values[12], values[13] + a.values[13], values[14] + a.values[14], values[15] + a.values[15]);
   }
   Mat4 operator-(Mat4 a)
   {
      return Mat4(values[0] - a.values[0], values[1] - a.values[1], values[2] - a.values[2], values[3] - a.values[3],
         values[4] - a.values[4], values[5] - a.values[5], values[6] - a.values[6], values[7] - a.values[7],
         values[8] - a.values[8], values[9] - a.values[9], values[10] - a.values[10], values[11] - a.values[11],
         values[12] - a.values[12], values[13] - a.values[13], values[14] - a.values[14], values[15] - a.values[15]);
   }
   Mat4& operator*=(T a)
   {
      for (int i = 0; i < 16; ++i) values[i] *= a;
      return *this;
   }
   Mat4& operator/=(T a)
   {
      for (int i = 0; i < 16; ++i) values[i] /= a;
      return *this;
   }
   Mat4& operator+=(T a)
   {
      for (int i = 0; i < 16; ++i) values[i] += a;
      return *this;
   }
   Mat4& operator-=(T a)
   {
      for (int i = 0; i < 16; ++i) values[i] -= a;
      return *this;
   }
   Mat4 operator-() const
   {
      Mat4 r(*this);
      for (int i = 0; i < 16; ++i) r.values[i] = -r.values[i];
      return r;
   }
   Mat4 inverse() const
   {
      T fA0 = values[ 0]*values[ 5] - values[ 1]*values[ 4];
      T fA1 = values[ 0]*values[ 6] - values[ 2]*values[ 4];
      T fA2 = values[ 0]*values[ 7] - values[ 3]*values[ 4];
      T fA3 = values[ 1]*values[ 6] - values[ 2]*values[ 5];
      T fA4 = values[ 1]*values[ 7] - values[ 3]*values[ 5];
      T fA5 = values[ 2]*values[ 7] - values[ 3]*values[ 6];
      T fB0 = values[ 8]*values[13] - values[ 9]*values[12];
      T fB1 = values[ 8]*values[14] - values[10]*values[12];
      T fB2 = values[ 8]*values[15] - values[11]*values[12];
      T fB3 = values[ 9]*values[14] - values[10]*values[13];
      T fB4 = values[ 9]*values[15] - values[11]*values[13];
      T fB5 = values[10]*values[15] - values[11]*values[14];

      T fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
      if (fabs(fDet) <= Math::cZERO_TOLERANCE)
      {
        return zero();
      }

      Mat4 kInv;
      kInv.values[ 0] =
        + values[ 5]*fB5 - values[ 6]*fB4 + values[ 7]*fB3;
      kInv.values[ 4] =
        - values[ 4]*fB5 + values[ 6]*fB2 - values[ 7]*fB1;
      kInv.values[ 8] =
        + values[ 4]*fB4 - values[ 5]*fB2 + values[ 7]*fB0;
      kInv.values[12] =
        - values[ 4]*fB3 + values[ 5]*fB1 - values[ 6]*fB0;
      kInv.values[ 1] =
        - values[ 1]*fB5 + values[ 2]*fB4 - values[ 3]*fB3;
      kInv.values[ 5] =
        + values[ 0]*fB5 - values[ 2]*fB2 + values[ 3]*fB1;
      kInv.values[ 9] =
        - values[ 0]*fB4 + values[ 1]*fB2 - values[ 3]*fB0;
      kInv.values[13] =
        + values[ 0]*fB3 - values[ 1]*fB1 + values[ 2]*fB0;
      kInv.values[ 2] =
        + values[13]*fA5 - values[14]*fA4 + values[15]*fA3;
      kInv.values[ 6] =
        - values[12]*fA5 + values[14]*fA2 - values[15]*fA1;
      kInv.values[10] =
        + values[12]*fA4 - values[13]*fA2 + values[15]*fA0;
      kInv.values[14] =
        - values[12]*fA3 + values[13]*fA1 - values[14]*fA0;
      kInv.values[ 3] =
        - values[ 9]*fA5 + values[10]*fA4 - values[11]*fA3;
      kInv.values[ 7] =
        + values[ 8]*fA5 - values[10]*fA2 + values[11]*fA1;
      kInv.values[11] =
        - values[ 8]*fA4 + values[ 9]*fA2 - values[11]*fA0;
      kInv.values[15] =
        + values[ 8]*fA3 - values[ 9]*fA1 + values[10]*fA0;

      T fInvDet = ((T)1.0)/fDet;
      kInv.values[ 0] *= fInvDet;
      kInv.values[ 1] *= fInvDet;
      kInv.values[ 2] *= fInvDet;
      kInv.values[ 3] *= fInvDet;
      kInv.values[ 4] *= fInvDet;
      kInv.values[ 5] *= fInvDet;
      kInv.values[ 6] *= fInvDet;
      kInv.values[ 7] *= fInvDet;
      kInv.values[ 8] *= fInvDet;
      kInv.values[ 9] *= fInvDet;
      kInv.values[10] *= fInvDet;
      kInv.values[11] *= fInvDet;
      kInv.values[12] *= fInvDet;
      kInv.values[13] *= fInvDet;
      kInv.values[14] *= fInvDet;
      kInv.values[15] *= fInvDet;

      return kInv;
   }


   Mat4 operator*(T b) const
   {
      Mat4 r(*this);
      r *= b;
      return r;
   }

   Mat4 operator/(T b) const
   {
      Mat4 r(*this);
      r /= b;
      return r;
   }

   Mat4 operator*(const Mat4& b) const
   {
      return Mat4(
        values[ 0]*b.values[ 0] +
        values[ 1]*b.values[ 4] +
        values[ 2]*b.values[ 8] +
        values[ 3]*b.values[12],

        values[ 0]*b.values[ 1] +
        values[ 1]*b.values[ 5] +
        values[ 2]*b.values[ 9] +
        values[ 3]*b.values[13],

        values[ 0]*b.values[ 2] +
        values[ 1]*b.values[ 6] +
        values[ 2]*b.values[10] +
        values[ 3]*b.values[14],

        values[ 0]*b.values[ 3] +
        values[ 1]*b.values[ 7] +
        values[ 2]*b.values[11] +
        values[ 3]*b.values[15],

        values[ 4]*b.values[ 0] +
        values[ 5]*b.values[ 4] +
        values[ 6]*b.values[ 8] +
        values[ 7]*b.values[12],

        values[ 4]*b.values[ 1] +
        values[ 5]*b.values[ 5] +
        values[ 6]*b.values[ 9] +
        values[ 7]*b.values[13],

        values[ 4]*b.values[ 2] +
        values[ 5]*b.values[ 6] +
        values[ 6]*b.values[10] +
        values[ 7]*b.values[14],

        values[ 4]*b.values[ 3] +
        values[ 5]*b.values[ 7] +
        values[ 6]*b.values[11] +
        values[ 7]*b.values[15],

        values[ 8]*b.values[ 0] +
        values[ 9]*b.values[ 4] +
        values[10]*b.values[ 8] +
        values[11]*b.values[12],

        values[ 8]*b.values[ 1] +
        values[ 9]*b.values[ 5] +
        values[10]*b.values[ 9] +
        values[11]*b.values[13],

        values[ 8]*b.values[ 2] +
        values[ 9]*b.values[ 6] +
        values[10]*b.values[10] +
        values[11]*b.values[14],

        values[ 8]*b.values[ 3] +
        values[ 9]*b.values[ 7] +
        values[10]*b.values[11] +
        values[11]*b.values[15],

        values[12]*b.values[ 0] +
        values[13]*b.values[ 4] +
        values[14]*b.values[ 8] +
        values[15]*b.values[12],

        values[12]*b.values[ 1] +
        values[13]*b.values[ 5] +
        values[14]*b.values[ 9] +
        values[15]*b.values[13],

        values[12]*b.values[ 2] +
        values[13]*b.values[ 6] +
        values[14]*b.values[10] +
        values[15]*b.values[14],

        values[12]*b.values[ 3] +
        values[13]*b.values[ 7] +
        values[14]*b.values[11] +
        values[15]*b.values[15]);
   }

   T values[16];
};

// Multiplication of a vector by a matrix (x,y,z,0) * M
inline Vec3 operator^(const Vec3& a, const Mat4& b)
{
   return Vec3(
      (a.x*b(0,0)+a.y*b(1,0)+a.z*b(2,0)+b(3,0)),
      (a.x*b(0,1)+a.y*b(1,1)+a.z*b(2,1)+b(3,1)),
      (a.x*b(0,2)+a.y*b(1,2)+a.z*b(2,2)+b(3,2)));
}
inline Vec3 operator^(const Mat4& b, const Vec3& a)
{
   return Vec3(
      (a.x*b(0,0)+a.y*b(1,0)+a.z*b(2,0)+b(3,0)),
      (a.x*b(0,1)+a.y*b(1,1)+a.z*b(2,1)+b(3,1)),
      (a.x*b(0,2)+a.y*b(1,2)+a.z*b(2,2)+b(3,2)));
}

// Multiplication of a point by a matrix (x,y,z,1) * M
inline Vec3 operator*(const Vec3& a, const Mat4& b)
{
   float w = a.x*b(0,3)+a.y*b(1,3)+a.z*b(2,3)+b(3,3);

   return Vec3(
      (a.x*b(0,0)+a.y*b(1,0)+a.z*b(2,0)+b(3,0)) / w,
      (a.x*b(0,1)+a.y*b(1,1)+a.z*b(2,1)+b(3,1)) / w,
      (a.x*b(0,2)+a.y*b(1,2)+a.z*b(2,2)+b(3,2)) / w);
}
// Multiplication of a point by a matrix (x,y,z,1) * M
inline Vec3 operator*(const Mat4& b, const Vec3& a)
{
   float w = a.x*b(0,3)+a.y*b(1,3)+a.z*b(2,3)+b(3,3);

   return Vec3(
      (a.x*b(0,0)+a.y*b(1,0)+a.z*b(2,0)+b(3,0)) / w,
      (a.x*b(0,1)+a.y*b(1,1)+a.z*b(2,1)+b(3,1)) / w,
      (a.x*b(0,2)+a.y*b(1,2)+a.z*b(2,2)+b(3,2)) / w);
}

inline Vec2 operator*(const Vec2& a, const Mat4& b)
{
   Vec3 X = Vec3(a.x, a.y, 0) * b;
   return Vec2(X.x, X.y);
}

inline Vec2 operator*(const Mat4& b, const Vec2& a)
{
   Vec3 X = Vec3(a.x, a.y, 0) * b;
   return Vec2(X.x, X.y);
}

namespace math
{
inline Mat4 rotation_x(float a)
{
   return Mat4(1,0,0,0, 0,cos(a),-sin(a),0, 0,sin(a),cos(a),0, 0,0,0,1);
}
inline Mat4 rotation_y(float a)
{
   return Mat4(cos(a), 0, sin(a), 0, 0,1,0,0, -sin(a), 0, cos(a), 0, 0,0,0,1);
}
inline Mat4 rotation_z(float a)
{
   return Mat4(cos(a), -sin(a), 0, 0, sin(a), cos(a),0,0,0,0,1,0,0,0,0,1);
}

inline Mat4 rotation(float aAngle, Vec3 aAxis)
{
   Vec3 v = aAxis.normal();
   float x = v.x;
   float y = v.y;
   float z = v.z;
   float c = cos(aAngle);
   float s = sin(aAngle);
   return Mat4(
      x*x*(1-c)+c,   x*y*(1-c)-z*s, x*z*(1-c)+y*s, 0,
      y*x*(1-c)+z*s, y*y*(1-c)+c,   y*z*(1-c)-x*s, 0,
      x*z*(1-c)-y*s, y*z*(1-c)+x*s, z*z*(1-c)+c,   0,
      0,             0,             0,             1).transpose();
}

inline Mat4 translate4(float x, float y, float z=0.0f)
{
   return Mat4(1, 0, 0, 0,
               0, 1, 0, 0,
               0, 0, 1, 0,
               x, y, z, 1);
}
inline Mat4 translate4(const Vec3& a) { return translate4(a.x, a.y, a.z); }
//inline Mat4 translate(const Vec2& a) { return translate(a.x, a.y); }
inline Mat4 scale4(float x, float y, float z=1.0f)
{
   return Mat4(x, 0, 0, 0,
               0, y, 0, 0,
               0, 0, z, 0,
               0, 0, 0, 1);
}
inline Mat4 scale4(const Vec3& a) { return scale4(a.x, a.y, a.z); }

inline Mat4 perspective(float fovy,
                        float aspect,
                        float zNear,
                        float zFar)
{
   float f = 1.0f / tan(fovy * 3.1415926f/180/2);
   return Mat4(f/aspect, 0, 0,                           0,
               0,        f, 0,                           0,
               0,        0, (zFar+zNear)/(zNear - zFar), 2*(zNear * zFar) / (zNear - zFar),
               0,        0, -1,                          0).transpose();
}

inline Mat4 look_at(Vec3 aEye,
                    Vec3 aCenter,
                    Vec3 aUp)
{
   Vec3 f = aCenter - aEye;
   f.normalize();
   Vec3 up = aUp.normal();
   Vec3 s = cross(f, up);
   Vec3 u = cross(s, f);
   return translate4(-aEye) * 
      Mat4(
      s[0],    s[1],    s[2], 0,
      u[0],    u[1],    u[2], 0,
      -f[0],   -f[1],  -f[2], 0,
      0,       0,       0,    1
      ).transpose();
}

}
#endif